#include "System.h"


WORD SteeringServoNeutralPosition;
WORD DriveServoNeutralPosition;
volatile BOOL ControlSystemTick;
RPMTickHolder PendingRPMMeasurement;
RPMTickHolder CurrentRPMMeasurement;

BOOL RPMMeasurementTimedOut;
DWORD CurrentRPM;

BOOL UpdateSteering;
BOOL UpdateDrive;
SIGNED_WORD QueuedSteeringCmd;
SIGNED_WORD QueuedDriveCmd;
static volatile WORD WaitCnt;
volatile WORD GPTimer[NUM_GP_TIMERS];

void InitRPMMeasurement()
{
   PendingRPMMeasurement.FullValue = 0;
   CurrentRPMMeasurement.FullValue = 0;
}

void InitServos()
{
     SteeringServoNeutralPosition = STEERING_SERVO_NEUTRAL;
     DriveServoNeutralPosition = DRIVE_SERVO_NEUTRAL;
     
     InitTimerTPM2();
     SetSteeringServo(0);
     SetDriveServo(0);
}

void SetSteeringServo(SIGNED_WORD ServoVal)
{
   ServoVal += SteeringServoNeutralPosition;
   if(ServoVal > STEERING_SERVO_MAX)
   {
      ServoVal =  STEERING_SERVO_MAX;
   }
   if(ServoVal < STEERING_SERVO_MIN)
   {
      ServoVal =  STEERING_SERVO_MIN;
   }		

  QueuedSteeringCmd = ServoVal;
  UpdateSteering = TRUE;
}

void SetDriveServo(SIGNED_WORD ServoVal)
{
   ServoVal += DriveServoNeutralPosition;
   if(ServoVal > DRIVE_SERVO_MAX)
   {
      ServoVal =  DRIVE_SERVO_MAX;
   }
   if(ServoVal < DRIVE_SERVO_MIN)
   {
      ServoVal =  DRIVE_SERVO_MIN;
   }

   QueuedDriveCmd = ServoVal;
   UpdateDrive = TRUE;		

}


volatile BYTE HostUpdateTimer;

BYTE CTick;

interrupt VectorNumber_Vtpm2ovf void TPM2_Overflow(void)
{

  BYTE i;
 
  ControlSystemTick = TRUE;

  for(i=0;i<NUM_GP_TIMERS;i++)
  {
   if(GPTimer[i] < 0xFFFF)
   {
      GPTimer[i]++;
   }
  }

  if(UpdateSteering == TRUE) 
  {
      TPM2C0V = (WORD)QueuedSteeringCmd; 
      UpdateSteering = FALSE;
  }
        
  if(UpdateDrive == TRUE) 
  {
      TPM2C1V = (WORD)QueuedDriveCmd;
      UpdateDrive = FALSE;
  }

  ButtonDetect();

  CaptureSensors();
  
 
  
  if(HostUpdateTimer < 0xFF)
  {
     HostUpdateTimer++;
  } 
 //T0 clear the interrupt two steps are needed!  (See Page 346
 //of the reference manual)
 
 //You first must read the Interrupt flag and then write a zero to 
 //clear it
  
   TPM2SC &= ~TPM2SC_TOF_MASK; 
}

  void InitTimerTPM2(void)
{
  
//Ensure the timer is reset
TPM2SC = 0;  

//Prescale factor is 8
TPM2SC_PS = 3;

TPM2MOD =  (WORD)(SERVO_REFRESH_TIME * (BUS_CLOCK/TPM2_PRESCALE_FACTOR));
 
//Set the clocks source to be the BUS Clock
TPM2SC |=  TPM1SC_CLKSA_MASK;
TPM2SC &= ~TPM1SC_CLKSB_MASK;
 
//Enable TPM1 Clock Gate
SCGC1 |= SCGC1_TPM2_MASK; 


TPM2C0SC |=  TPM2C0SC_ELS0B_MASK;
TPM2C0SC &=  ~TPM2C0SC_ELS0A_MASK; 
TPM2C0SC |=  TPM2C0SC_MS0B_MASK;

TPM2C1SC |=  TPM2C1SC_ELS1B_MASK;
TPM2C1SC &=  ~TPM2C1SC_ELS1A_MASK; 
TPM2C1SC |=  TPM2C1SC_MS1B_MASK;

//enable Interrupts

TPM2SC |=  TPM2SC_TOIE_MASK;

ControlSystemTick = FALSE;

}


void InitTimerTPM1()
{
  
//Ensure the timer is reset
TPM1SC = 0; 

TPM2SC_PS = 0;

//Set the overflow rate
//Full count to 65k
TPM1MOD = 0;
 
//Set the clocks source to be the BUS Clock
TPM1SC |=  TPM1SC_CLKSA_MASK;
TPM1SC &= ~TPM1SC_CLKSB_MASK;
 
//Enable TPM1 Clock Gate
SCGC1 |= SCGC1_TPM1_MASK; 

//enable Interrupts  for overflow
TPM1SC |=  TPM1SC_TOIE_MASK;

//Set Input Caputre for Falling Edge
TPM1C0SC =  TPM2C0SC_ELS0B_MASK;
  
//Enable interrupts
TPM1C0SC |=  TPM2C0SC_CH0IE_MASK;

}



interrupt VectorNumber_Vtpm1ch0 void InputCaputureTMP1CH0_IRQ(void)
{
    if( RPMMeasurementTimedOut == TRUE)
    {
        RPMMeasurementTimedOut = FALSE;
        CurrentRPMMeasurement.FullValue = 0;
        PendingRPMMeasurement.FullValue = 0;
        TPM1CNT = 0;
    }
    else
    {
         PendingRPMMeasurement.Pieces.Low =  TPM1C0V;
         CurrentRPMMeasurement.FullValue =  PendingRPMMeasurement.FullValue;
         PendingRPMMeasurement.FullValue = 0;
         TPM1CNT =0;
    }
          
    TPM1C0SC &= ~TPM1C0SC_CH0F_MASK;
} 


static volatile WORD WaitCnt;


interrupt VectorNumber_Vtpm1ovf void TPM1_Overflow(void)
{
   
   if(RPMMeasurementTimedOut == FALSE)
   {
   
     PendingRPMMeasurement.Pieces.Hi++;
     
     if(PendingRPMMeasurement.Pieces.Hi>=MAX_RPM_TIMEOUT_COUNT)
     {
          RPMMeasurementTimedOut = TRUE;
          PendingRPMMeasurement.FullValue = 0;
          CurrentRPMMeasurement.FullValue = 0;
     }
   }
   

 //T0 clear the interrupt two steps are needed!  (See Page 346
 //of the reference manual)
 
 //You first must read the Interrupt flag and then write a zero to 
 //clear it
  
   TPM1SC &= ~TPM1SC_TOF_MASK; 
}





void Block(WORD Ticks)
{
  
 WaitCnt = 0;
 
   while(WaitCnt<Ticks)
   {
   }
    
}


